%IMU的采样评率为50HZ
%将IMU与UWB数据融合
[sizeof_t_UWB,~]=size(t_relative_UWB);
[sizeof_t_IMU,~]=size(t_relative_IMU);
accleration_x_adj=zeros(sizeof_t_UWB,1);
accleration_y_adj=zeros(sizeof_t_UWB,1);
accleration_z_adj=zeros(sizeof_t_UWB,1);
%利用这个求出IMU最大时间间隔0.034
% erroroftime_max=0;
% for i=1:sizeof_t_IMU-1
%     erroroftime=t_relative_IMU(i+1)-t_relative_IMU(i);
%     if erroroftime>erroroftime_max
%         erroroftime_max=erroroftime;
%     end
% end
flag_j=1;
for i=1:sizeof_t_UWB
   for j=flag_j:sizeof_t_IMU
       erroroftime=abs(t_relative_UWB(i)-t_relative_IMU(j));
       if erroroftime<0.017
           accleration_x_adj(i)=accleration_x(j);
           accleration_y_adj(i)=accleration_y(j);
           accleration_z_adj(i)=accleration_z(j);
           flag_j=j+1;
           continue;
       end
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u_IMU(1,:)=accleration_x_adj;
u_IMU(2,:)=accleration_y_adj;
u_IMU(3,:)=accleration_z_adj-9.8;
delt_t=0.04;
A_IMU=[1 0 0 delt_t 0 0;
       0 1 0 0 delt_t 0;
       0 0 1 0 0 delt_t;
       0 0 0 1 0 0;
       0 0 0 0 1 0;
       0 0 0 0 0 1];
B_IMU=[0.5*delt_t^2 0 0;
       0 0.5*delt_t^2 0;
       0 0 0.5*delt_t^2;
       delt_t 0 0;
       0 delt_t 0;
       0 0 delt_t];
Pos_IMU=zeros(6,sizeof_t_UWB);
M_IMU=zeros(6,3);
for i=1:sizeof_t_UWB
   if i==1
      Pos_IMU(1,i)=x_off;
      Pos_IMU(2,i)=y_off;
      Pos_IMU(3,i)=z_off;
   else
       dis0_IMU=sqrt((Pos_IMU(1,i-1)-x0)^2+(Pos_IMU(2,i-1)-y0)^2+(Pos_IMU(3,i-1)-z0)^2);
       dis1_IMU=sqrt((Pos_IMU(1,i-1)-x1)^2+(Pos_IMU(2,i-1)-y1)^2+(Pos_IMU(3,i-1)-z1)^2);
       dis3_IMU=sqrt((Pos_IMU(1,i-1)-x3)^2+(Pos_IMU(2,i-1)-y3)^2+(Pos_IMU(3,i-1)-z3)^2);
       M_IMU(1,1)=(Pos_IMU(1,i-1)-x0)/dis0_IMU;
       M_IMU(2,1)=(Pos_IMU(2,i-1)-y0)/dis0_IMU;
       M_IMU(3,1)=(Pos_IMU(3,i-1)-z0)/dis0_IMU;
       M_IMU(1,2)=(Pos_IMU(1,i-1)-x1)/dis1_IMU;
       M_IMU(2,2)=(Pos_IMU(2,i-1)-y1)/dis1_IMU;
       M_IMU(3,2)=(Pos_IMU(3,i-1)-z1)/dis1_IMU;
       M_IMU(1,3)=(Pos_IMU(1,i-1)-x3)/dis3_IMU;
       M_IMU(2,3)=(Pos_IMU(2,i-1)-y3)/dis3_IMU;
       M_IMU(3,3)=(Pos_IMU(3,i-1)-z3)/dis3_IMU;
       Pos_IMU(:,i)=A_IMU* Pos_IMU(:,i-1)+B_IMU*u_IMU(:,i)+M_IMU(:,1)*(dis_sg(i-1,1)-dis0_IMU)...
       +M_IMU(:,2)*(dis_sg(i-1,2)-dis1_IMU)+M_IMU(:,3)*(dis_sg(i-1,3)-dis3_IMU); 
%        Pos_IMU(:,i)=A_IMU* Pos_IMU(:,i-1)+B_IMU*u_IMU(:,i)+M_IMU(:,3)*(dis_sg(i-1,3)-dis3_IMU)+M_IMU(:,1)*(dis_sg(i-1,1)-dis0_IMU);
%          Pos_IMU(:,i)=A_IMU* Pos_IMU(:,i-1)+B_IMU*u_IMU(:,i)+M_IMU(:,3)*(dis_sg(i-1,3)-dis3_IMU);
   end
end
Pos_IMU_T=Pos_IMU';
Pos_IMU_sg_T=sgolayfilt(Pos_IMU_T,3,11);
Pos_IMU_sg=Pos_IMU_sg_T';